﻿#region Using Statements
using System;
using System.Collections.Generic;
using System.Text;
using Microsoft.Xna.Framework;
using starLiGHT;

#endregion

#region COPYRIGHT

/*
    Copyright (c) 2008, 2009, 2010 
        Roland Rosenkranz (Glatzemann@email.de)
*/

#endregion

#region LICENSE

/*
    This file is part of starLiGHT.Collision.

    starLiGHT.Collision is free software: you can redistribute it and/or modify
    it under the terms of the GNU Lesser General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    starLiGHT.Collision is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU Lesser General Public License for more details.

    You should have received a copy of the GNU Lesser General Public License
    along with starLiGHT.Collision.  If not, see <http://www.gnu.org/licenses/>.

    
    ADDITIONAL (commercial) LICENSES for starLiGHT.Collision are available on request.
*/

#endregion

#region Box2D Copyright
/*
* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty.  In no event will the authors be held liable for any damages
* arising from the use of this software.
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
* 3. This notice may not be removed or altered from any source distribution.
*/
#endregion

#region Version Stuff
// **************[ starLiGHT.Engine SVN ]**********************
// * $Rev:: 1486                  $:  Revision of last commit *
// * $Author:: glatzemann         $:  Author of last commit   *
// * $Date:: 2010-10-08 15:55:47 #$:  Date of last commit     *
// ************************************************************

// **************[ Box2D-Repository Info ]*********************
// Header-File: http://code.google.com/p/box2d/source/browse/trunk/Box2D/Box2D/Dynamics/Contacts/b2ContactSolver.h
// Revision   : r116
// Change-Date: 2010-06-21
//
// Source-File: http://code.google.com/p/box2d/source/browse/trunk/Box2D/Box2D/Dynamics/Contacts/b2ContactSolver.cpp
// Revision   : r116
// Change-Date: 2010-06-21
//
// Status     : Finished!
#endregion

namespace starLiGHT.Physic.TwoD
{
    public struct ContactConstraintPoint
    {
	    public Vector2 localPoint;
	    public Vector2 rA;
	    public Vector2 rB;
	    public float normalImpulse;
	    public float tangentImpulse;
	    public float normalMass;
	    public float tangentMass;
	    public float velocityBias;
    };

    public struct ContactConstraint
    {
        public ContactConstraintPoint[] points;
        public Vector2 localNormal;
        public Vector2 localPoint;
        public Vector2 normal;
        public Mat22 normalMass;
        public Mat22 K;
        public Body bodyA;
        public Body bodyB;
        public ManifoldType type;
        public float radiusA, radiusB;
        public float friction;
        public float restitution;
        public int pointCount;
        public Manifold manifold;
    };

    public struct ContactSolverDef
    {
        public Contact[] contacts;
        public int count;
        public float impulseRatio;
        public bool warmStarting;
    }

    public class ContactSolver
    {
        WorldManifold worldManifold = new WorldManifold();

        public ContactSolver()
        {
        }

        public void Reset(ref ContactSolverDef def)
        {
            m_count = def.count;

            // grow the array
            if (m_constraints == null || m_constraints.Length <= m_count)
            {
                m_constraints = new ContactConstraint[m_count * 2];
            }

            // Initialize position independent portions of the constraints.
            for (Int32 i = 0; i < m_count; ++i)
            {
                Contact contact = def.contacts[i];

                Fixture fixtureA = contact.m_fixtureA;
                Fixture fixtureB = contact.m_fixtureB;
                Shape shapeA = fixtureA.GetShape();
                Shape shapeB = fixtureB.GetShape();
                float radiusA = shapeA.m_radius;
                float radiusB = shapeB.m_radius;
                Body bodyA = fixtureA.GetBody();
                Body bodyB = fixtureB.GetBody();
                
                Manifold manifold;
                contact.GetManifold(out manifold);

                System.Diagnostics.Debug.Assert(manifold.pointCount > 0);

                ContactConstraint cc = m_constraints[i];
                cc.points = new ContactConstraintPoint[2];

                cc.friction = PhysicSettings.MixFriction(fixtureA.GetFriction(), fixtureB.GetFriction());
                cc.restitution = PhysicSettings.MixRestitution(fixtureA.GetRestitution(), fixtureB.GetRestitution());
                cc.bodyA = bodyA;
                cc.bodyB = bodyB;
                cc.manifold = manifold;
                cc.normal = Vector2.Zero;
                cc.pointCount = manifold.pointCount;

                cc.localNormal = manifold.localNormal;
                cc.localPoint = manifold.localPoint;

                cc.radiusA = radiusA;
                cc.radiusB = radiusB;
                cc.type = manifold.type;

                for (Int32 j = 0; j < cc.pointCount; ++j)
                {
                    ManifoldPoint cp = manifold.points[j];
                    ContactConstraintPoint ccp = cc.points[j];

                    if (def.warmStarting)
                    {
                        ccp.normalImpulse = def.impulseRatio * cp.normalImpulse;
                        ccp.tangentImpulse = def.impulseRatio * cp.tangentImpulse;
                    }
                    else
                    {
                        ccp.normalImpulse = 0.0f;
                        ccp.tangentImpulse = 0.0f;
                    }

                    ccp.localPoint = cp.localPoint;
                    ccp.rA = Vector2.Zero;
                    ccp.rB = Vector2.Zero;
                    ccp.normalMass = 0.0f;
                    ccp.tangentMass = 0.0f;
                    ccp.velocityBias = 0.0f;

                    cc.points[j] = ccp;
                    manifold.points[j] = cp;
                }

                cc.K.SetZero();
                cc.normalMass.SetZero();

                m_constraints[i] = cc;
                //def.contacts[i] = contact;  // contact is a class -> no write back necessary
            }
        }

        public void InitializeVelocityConstraints()
        {
            for (int i = 0; i < m_count; ++i)
            {
                ContactConstraint cc = m_constraints[i];

                float radiusA = cc.radiusA;
                float radiusB = cc.radiusB;
                Body bodyA = cc.bodyA;
                Body bodyB = cc.bodyB;
                Manifold manifold = cc.manifold;

                Vector2 vA = bodyA.m_linearVelocity;
                Vector2 vB = bodyB.m_linearVelocity;
                float wA = bodyA.m_angularVelocity;
                float wB = bodyB.m_angularVelocity;

                System.Diagnostics.Debug.Assert(manifold.pointCount > 0);

                WorldManifold worldManifold = new WorldManifold();
                worldManifold.Initialize(manifold, bodyA.m_xf, radiusA, bodyB.m_xf, radiusB);

                cc.normal = worldManifold.normal;

                for (int j = 0; j < cc.pointCount; ++j)
                {
                    ManifoldPoint cp = manifold.points[j];
                    ContactConstraintPoint ccp = cc.points[j];

                    ccp.rA = worldManifold.points[j] - bodyA.m_sweep.c;
                    ccp.rB = worldManifold.points[j] - bodyB.m_sweep.c;

                    float rnA = Vector2Extensions.Cross(ccp.rA, cc.normal);
                    float rnB = Vector2Extensions.Cross(ccp.rB, cc.normal);
                    rnA *= rnA;
                    rnB *= rnB;

                    float kNormal = bodyA.m_invMass + bodyB.m_invMass + bodyA.m_invI * rnA + bodyB.m_invI * rnB;

                    System.Diagnostics.Debug.Assert(kNormal > PhysicSettings.FLT_EPSILON);
                    ccp.normalMass = 1.0f / kNormal;

                    Vector2 tangent = Vector2Extensions.Cross(cc.normal, 1.0f);

                    float rtA = Vector2Extensions.Cross(ccp.rA, tangent);
                    float rtB = Vector2Extensions.Cross(ccp.rB, tangent);
                    rtA *= rtA;
                    rtB *= rtB;

                    float kTangent = bodyA.m_invMass + bodyB.m_invMass + bodyA.m_invI * rtA + bodyB.m_invI * rtB;

                    System.Diagnostics.Debug.Assert(kTangent > PhysicSettings.FLT_EPSILON);
                    ccp.tangentMass = 1.0f / kTangent;

                    // Setup a velocity bias for restitution.
                    ccp.velocityBias = 0.0f;
                    float vRel = Vector2.Dot(cc.normal, vB + Vector2Extensions.Cross(wB, ccp.rB) - vA - Vector2Extensions.Cross(wA, ccp.rA));
                    if (vRel < -PhysicSettings.VelocityThreshold)
                    {
                        ccp.velocityBias = -cc.restitution * vRel;
                    }

                    cc.points[j] = ccp;
                    manifold.points[j] = cp;
                }

                // If we have two points, then prepare the block solver.
                if (cc.pointCount == 2)
                {
                    ContactConstraintPoint ccp1 = cc.points[0];
                    ContactConstraintPoint ccp2 = cc.points[1];

                    float invMassA = bodyA.m_invMass;
                    float invIA = bodyA.m_invI;
                    float invMassB = bodyB.m_invMass;
                    float invIB = bodyB.m_invI;

                    float rn1A = Vector2Extensions.Cross(ccp1.rA, cc.normal);
                    float rn1B = Vector2Extensions.Cross(ccp1.rB, cc.normal);
                    float rn2A = Vector2Extensions.Cross(ccp2.rA, cc.normal);
                    float rn2B = Vector2Extensions.Cross(ccp2.rB, cc.normal);

                    float k11 = invMassA + invMassB + invIA * rn1A * rn1A + invIB * rn1B * rn1B;
                    float k22 = invMassA + invMassB + invIA * rn2A * rn2A + invIB * rn2B * rn2B;
                    float k12 = invMassA + invMassB + invIA * rn1A * rn2A + invIB * rn1B * rn2B;

                    // Ensure a reasonable condition number.
                    const float k_maxConditionNumber = 100.0f;
                    if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12))
                    {
                        // K is safe to invert.
                        cc.K.col1 = new Vector2(k11, k12);
                        cc.K.col2 = new Vector2(k12, k22);
                        //cc.K.col1 = new Vector2(); cc.K.col1.X = k11; cc.K.col1.Y = k12;
                        //cc.K.col2 = new Vector2(); cc.K.col2.X = k12; cc.K.col2.Y = k22;
                        cc.normalMass = cc.K.GetInverse();
                    }
                    else
                    {
                        // The constraints are redundant, just use one.
                        // TODO_ERIN use deepest?
                        cc.pointCount = 1;
                    }

                    cc.points[0] = ccp1;
                    cc.points[1] = ccp2;
                }

                m_constraints[i] = cc;
            }
        }

        public void WarmStart()
        {
            // Warm start.
            for (int i = 0; i < m_count; ++i)
            {
                ContactConstraint c = m_constraints[i];

                Body bodyA = c.bodyA;
                Body bodyB = c.bodyB;
                float invMassA = bodyA.m_invMass;
                float invIA = bodyA.m_invI;
                float invMassB = bodyB.m_invMass;
                float invIB = bodyB.m_invI;
                Vector2 normal = c.normal;
                Vector2 tangent = Vector2Extensions.Cross(normal, 1.0f);

                for (int j = 0; j < c.pointCount; ++j)
                {
                    ContactConstraintPoint ccp = c.points[j];
                    Vector2 P = ccp.normalImpulse * normal + ccp.tangentImpulse * tangent;
                    bodyA.m_angularVelocity -= invIA * Vector2Extensions.Cross(ccp.rA, P);
                    bodyA.m_linearVelocity -= invMassA * P;
                    bodyB.m_angularVelocity += invIB * Vector2Extensions.Cross(ccp.rB, P);
                    bodyB.m_linearVelocity += invMassB * P;
                    c.points[j] = ccp;
                }

                m_constraints[i] = c;
            }
        }

        public void SolveVelocityConstraints()
        {
            ContactConstraint c;
            Body bodyA, bodyB;

	        for (int i = 0; i < m_count; ++i)
	        {
		        c = m_constraints[i];
		        bodyA = c.bodyA;
		        bodyB = c.bodyB;
		        float wA = bodyA.m_angularVelocity;
		        float wB = bodyB.m_angularVelocity;
		        Vector2 vA = bodyA.m_linearVelocity;
		        Vector2 vB = bodyB.m_linearVelocity;
		        float invMassA = bodyA.m_invMass;
		        float invIA = bodyA.m_invI;
		        float invMassB = bodyB.m_invMass;
		        float invIB = bodyB.m_invI;
		        Vector2 normal = c.normal;
#if !OPTIMIZE
		        Vector2 tangent = Vector2Extensions.Cross(normal, 1.0f);
#else
                Vector2 tangent; tangent.X = normal.Y; tangent.Y = -normal.X;
#endif
		        float friction = c.friction;

                System.Diagnostics.Debug.Assert(c.pointCount == 1 || c.pointCount == 2);

		        // Solve tangent constraints
		        for (int j = 0; j < c.pointCount; ++j)
		        {
			        ContactConstraintPoint ccp = c.points[j];

			        // Relative velocity at contact
#if !OPTIMIZE
                    Vector2 dv = vB + Vector2Extensions.Cross(wB, ccp.rB) - vA - Vector2Extensions.Cross(wA, ccp.rA);
#else
                    Vector2 dv; dv.X = vB.X + (-wB * ccp.rB.Y) - vA.X - (-wA * ccp.rA.Y); dv.Y = vB.Y + (wB * ccp.rB.X) - vA.Y - (wA * ccp.rA.X);
#endif

			        // Compute tangent force
#if !OPTIMIZE
                    float vt = Vector2.Dot(dv, tangent);
#else
                    float vt = dv.X * tangent.X + dv.Y * tangent.Y; // Vector2.Dot(ref dv, ref tangent, out vt);
#endif
                    float lambda = ccp.tangentMass * (-vt);

			        // b2Clamp the accumulated force
			        float maxFriction = friction * ccp.normalImpulse;
			        float newImpulse = MathHelper.Clamp(ccp.tangentImpulse + lambda, -maxFriction, maxFriction);
			        lambda = newImpulse - ccp.tangentImpulse;
                    
			        // Apply contact impulse
#if !OPTIMIZE
                    Vector2 P = lambda * tangent;
#else
                    Vector2 P; P.X = lambda * tangent.X; P.Y = lambda * tangent.Y;
#endif

#if !OPTIMIZE
                    vA -= invMassA * P;
			        wA -= invIA * Vector2Extensions.Cross(ccp.rA, P);
#else
                    vA.X = vA.X - invMassA * P.X; vA.Y = vA.Y - invMassA * P.Y;
                    wA = wA - invIA * (ccp.rA.X * P.Y - ccp.rA.Y * P.X);
#endif

#if !OPTIMIZE
                    vB += invMassB * P;
			        wB += invIB * Vector2Extensions.Cross(ccp.rB, P);
#else
                    vB.X = vB.X + invMassB * P.X; vB.Y = vB.Y + invMassB * P.Y;
                    wB = wB + invIB * (ccp.rB.X * P.Y - ccp.rB.Y * P.X);
#endif

			        ccp.tangentImpulse = newImpulse;

                    c.points[j] = ccp;
		        }

		        // Solve normal constraints
		        if (c.pointCount == 1)
		        {
			        ContactConstraintPoint ccp = c.points[0];

			        // Relative velocity at contact
#if !OPTIMIZE
			        Vector2 dv = vB + Vector2Extensions.Cross(wB, ccp.rB) - vA - Vector2Extensions.Cross(wA, ccp.rA);
#else
                    Vector2 dv; dv.X = vB.X + (-wB * ccp.rB.Y) - vA.X - (-wA * ccp.rA.Y); dv.Y = vB.Y + (wB * ccp.rB.X) - vA.Y - (wA * ccp.rA.X);
#endif

			        // Compute normal impulse
#if !OPTIMIZE
			        float vn = Vector2.Dot(dv, normal);
#else
                    float vn = dv.X * normal.X + dv.Y * normal.Y;  // Vector2.Dot(ref dv, ref normal, out vn);
#endif
			        float lambda = -ccp.normalMass * (vn - ccp.velocityBias);

			        // b2Clamp the accumulated impulse
                    float newImpulse = System.Math.Max(ccp.normalImpulse + lambda, 0.0f);
			        lambda = newImpulse - ccp.normalImpulse;

			        // Apply contact impulse
#if !OPTIMIZE
                    Vector2 P = lambda * normal;
			        vA -= invMassA * P;
			        wA -= invIA * Vector2Extensions.Cross(ccp.rA, P);

			        vB += invMassB * P;
			        wB += invIB * Vector2Extensions.Cross(ccp.rB, P);
#else
                    Vector2 P; P.X = lambda * normal.X; P.Y = lambda * normal.Y;
                    vA.X = vA.X - invMassA * P.X; vA.Y = vA.Y - invMassA * P.Y;
                    wA = wA - invIA * (ccp.rA.X * P.Y - ccp.rA.Y * P.X);

                    vB.X = vB.X + invMassB * P.X; vB.Y = vB.Y + invMassB * P.Y;
                    wB = wB + invIB * (ccp.rB.X * P.Y - ccp.rB.Y * P.X);
#endif
			        
                    ccp.normalImpulse = newImpulse;
                    c.points[0] = ccp;
		        }
		        else
		        {
			        // Block solver developed in collaboration with Dirk Gregorius (back in 01/07 on Box2D_Lite).
			        // Build the mini LCP for this contact patch
			        //
			        // vn = A * x + b, vn >= 0, , vn >= 0, x >= 0 and vn_i * x_i = 0 with i = 1..2
			        //
			        // A = J * W * JT and J = (-n, -r1 x n, n, r2 x n)
			        // b = vn_0 - velocityBias
			        //
			        // The system is solved using the "Total enumeration method" (s. Murty). The complementary constraint vn_i * x_i
			        // implies that we must have in any solution either vn_i = 0 or x_i = 0. So for the 2D contact problem the cases
			        // vn1 = 0 and vn2 = 0, x1 = 0 and x2 = 0, x1 = 0 and vn2 = 0, x2 = 0 and vn1 = 0 need to be tested. The first valid
			        // solution that satisfies the problem is chosen.
			        // 
			        // In order to account of the accumulated impulse 'a' (because of the iterative nature of the solver which only requires
			        // that the accumulated impulse is clamped and not the incremental impulse) we change the impulse variable (x_i).
			        //
			        // Substitute:
			        // 
			        // x = x' - a
			        // 
			        // Plug into above equation:
			        //
			        // vn = A * x + b
			        //    = A * (x' - a) + b
			        //    = A * x' + b - A * a
			        //    = A * x' + b'
			        // b' = b - A * a;

			        ContactConstraintPoint cp1 = c.points[0];
			        ContactConstraintPoint cp2 = c.points[1];

#if !OPTIMIZE
			        Vector2 a = new Vector2(cp1.normalImpulse, cp2.normalImpulse);
#else
                    Vector2 a; a.X = cp1.normalImpulse; a.Y = cp2.normalImpulse;
#endif
                    System.Diagnostics.Debug.Assert(a.X >= 0.0f && a.Y >= 0.0f);

			        // Relative velocity at contact
#if !OPTIMIZE
			        Vector2 dv1 = vB + Vector2Extensions.Cross(wB, cp1.rB) - vA - Vector2Extensions.Cross(wA, cp1.rA);
			        Vector2 dv2 = vB + Vector2Extensions.Cross(wB, cp2.rB) - vA - Vector2Extensions.Cross(wA, cp2.rA);
#else
                    Vector2 dv1;
                    Vector2 dv2;
                    dv1.X = vB.X + (-wB * cp1.rB.Y) - vA.X - (-wA * cp1.rA.Y);
                    dv1.Y = vB.Y + (wB * cp1.rB.X) - vA.Y - (wA * cp1.rA.X);
                    dv2.X = vB.X + (-wB * cp2.rB.Y) - vA.X - (-wA * cp2.rA.Y);
                    dv2.Y = vB.Y + (wB * cp2.rB.X) - vA.Y - (wA * cp2.rA.X);
#endif

			        // Compute normal velocity
#if !OPTIMIZE
			        float vn1 = Vector2.Dot(dv1, normal);
			        float vn2 = Vector2.Dot(dv2, normal);
                    Vector2 b; b.X = vn1 - cp1.velocityBias; b.Y = vn2 - cp2.velocityBias;
			        b = b - Math.Mul(c.K, a);
#else
                    float vn1 = dv1.X * normal.X + dv1.Y * normal.Y; // Vector2.Dot(ref dv1, ref normal, out vn1);
                    float vn2 = dv2.X * normal.X + dv2.Y * normal.Y; // Vector2.Dot(ref dv2, ref normal, out vn2);
                    Vector2 b; b.X = vn1 - cp1.velocityBias; b.Y = vn2 - cp2.velocityBias;
                    Vector2 t;
                    t.X = c.K.col1.X * a.X + c.K.col2.X * a.Y;
                    t.Y = c.K.col1.Y * a.X + c.K.col2.Y * a.Y;
                    b.X = b.X - t.X;
                    b.Y = b.Y - t.Y;
#endif

                    //NOTUSED: const float k_errorTol = 1e-3f;

			        for (;;)
			        {
				        //
				        // Case 1: vn = 0
				        //
				        // 0 = A * x' + b'
				        //
				        // Solve for x':
				        //
				        // x' = - inv(A) * b'
				        //
#if !OPTIMIZE
				        Vector2 x = - Math.Mul(c.normalMass, b);
#else
                        Vector2 x;
                        x.X = -(c.normalMass.col1.X * b.X + c.normalMass.col2.X * b.Y);
                        x.Y = -(c.normalMass.col1.Y * b.X + c.normalMass.col2.Y * b.Y);
#endif

				        if (x.X >= 0.0f && x.Y >= 0.0f)
				        {
					        // Resubstitute for the incremental impulse
#if !OPTIMIZE
					        Vector2 d = x - a;
#else
                            Vector2 d; d.X = x.X - a.X; d.Y = x.Y - a.Y;
#endif

					        // Apply incremental impulse
#if !OPTIMIZE
					        Vector2 P1 = d.X * normal;
					        Vector2 P2 = d.Y * normal;
					        vA -= invMassA * (P1 + P2);
					        wA -= invIA * (Vector2Extensions.Cross(cp1.rA, P1) + Vector2Extensions.Cross(cp2.rA, P2));

					        vB += invMassB * (P1 + P2);
					        wB += invIB * (Vector2Extensions.Cross(cp1.rB, P1) + Vector2Extensions.Cross(cp2.rB, P2));
#else
                            Vector2 P1; P1.X = d.X * normal.X; P1.Y = d.X * normal.Y;
                            Vector2 P2; P2.X = d.Y * normal.X; P2.Y = d.Y * normal.Y;
                            Vector2 P1P2; P1P2.X = P1.X + P2.X; P1P2.Y = P1.Y + P2.Y;
                            vA.X = vA.X - invMassA * P1P2.X; vA.Y = vA.Y - invMassA * P1P2.Y;
                            wA = wA - invIA * ((cp1.rA.X * P1.Y - cp1.rA.Y * P1.X) + (cp2.rA.X * P2.Y - cp2.rA.Y * P2.X));

                            vB.X = vB.X + invMassB * P1P2.X; vB.Y = vB.Y + invMassB * P1P2.Y;
                            wB = wB + invIB * ((cp1.rB.X * P1.Y - cp1.rB.Y * P1.X) + (cp2.rB.X * P2.Y - cp2.rB.Y * P2.X));
#endif

                            // Accumulate
					        cp1.normalImpulse = x.X;
					        cp2.normalImpulse = x.Y;

        #if B2_DEBUG_SOLVER
					        // Postconditions
					        dv1 = vB + Vector2Extensions.Cross(wB, cp1.rB) - vA - Vector2Extensions.Cross(wA, cp1.rA);
					        dv2 = vB + Vector2Extensions.Cross(wB, cp2.rB) - vA - Vector2Extensions.Cross(wA, cp2.rA);

					        // Compute normal velocity
					        vn1 = b2Dot(dv1, normal);
					        vn2 = b2Dot(dv2, normal);

					        System.Diagnostics.Debug.Assert(b2Abs(vn1 - cp1.velocityBias) < k_errorTol);
					        System.Diagnostics.Debug.Assert(b2Abs(vn2 - cp2.velocityBias) < k_errorTol);
        #endif
					        break;
				        }

				        //
				        // Case 2: vn1 = 0 and x2 = 0
				        //
				        //   0 = a11 * x1' + a12 * 0 + b1' 
				        // vn2 = a21 * x1' + a22 * 0 + b2'
				        //
				        x.X = -cp1.normalMass * b.X;
				        x.Y = 0.0f;
				        vn1 = 0.0f;
				        vn2 = c.K.col1.Y * x.X + b.Y;

				        if (x.X >= 0.0f && vn2 >= 0.0f)
				        {
					        // Resubstitute for the incremental impulse
#if !OPTIMIZE
                            Vector2 d = x - a;
#else
                            Vector2 d; d.X = x.X - a.X; d.Y = x.Y - a.Y;
#endif

					        // Apply incremental impulse
#if !OPTIMIZE
                            Vector2 P1 = d.X * normal;
					        Vector2 P2 = d.Y * normal;
                            vA -= invMassA * (P1 + P2);
                            wA -= invIA * (Vector2Extensions.Cross(cp1.rA, P1) + Vector2Extensions.Cross(cp2.rA, P2));

                            vB += invMassB * (P1 + P2);
                            wB += invIB * (Vector2Extensions.Cross(cp1.rB, P1) + Vector2Extensions.Cross(cp2.rB, P2));
#else
                            Vector2 P1; P1.X = d.X * normal.X; P1.Y = d.X * normal.Y;
                            Vector2 P2; P2.X = d.Y * normal.X; P2.Y = d.Y * normal.Y;
                            Vector2 P1P2; P1P2.X = P1.X + P2.X; P1P2.Y = P1.Y + P2.Y;
                            vA.X = vA.X - invMassA * P1P2.X; vA.Y = vA.Y - invMassA * P1P2.Y;
                            wA -= invIA * ((cp1.rA.X * P1.Y - cp1.rA.Y * P1.X) + (cp2.rA.X * P2.Y - cp2.rA.Y * P2.X));

                            vB.X = vB.X + invMassB * P1P2.X; vB.Y = vB.Y + invMassB * P1P2.Y;
                            wB += invIB * ((cp1.rB.X * P1.Y - cp1.rB.Y * P1.X) + (cp2.rB.X * P2.Y - cp2.rB.Y * P2.X));
#endif
                            
                            // Accumulate
					        cp1.normalImpulse = x.X;
					        cp2.normalImpulse = x.Y;

        #if B2_DEBUG_SOLVER
					        // Postconditions
					        dv1 = vB + Vector2Extensions.Cross(wB, cp1.rB) - vA - Vector2Extensions.Cross(wA, cp1.rA);

					        // Compute normal velocity
					        vn1 = b2Dot(dv1, normal);

					        System.Diagnostics.Debug.Assert(b2Abs(vn1 - cp1.velocityBias) < k_errorTol);
        #endif

					        break;
				        }


				        //
				        // Case 3: vn2 = 0 and x1 = 0
				        //
				        // vn1 = a11 * 0 + a12 * x2' + b1' 
				        //   0 = a21 * 0 + a22 * x2' + b2'
				        //
				        x.X = 0.0f;
				        x.Y = - cp2.normalMass * b.Y;
				        vn1 = c.K.col2.X * x.Y + b.X;
				        vn2 = 0.0f;

				        if (x.Y >= 0.0f && vn1 >= 0.0f)
				        {
					        // Resubstitute for the incremental impulse
#if !OPTIMIZE
                            Vector2 d = x - a;

					        // Apply incremental impulse
					        Vector2 P1 = d.X * normal;
					        Vector2 P2 = d.Y * normal;

                            vA -= invMassA * (P1 + P2);
                            wA -= invIA * (Vector2Extensions.Cross(cp1.rA, P1) + Vector2Extensions.Cross(cp2.rA, P2));

                            vB += invMassB * (P1 + P2);
                            wB += invIB * (Vector2Extensions.Cross(cp1.rB, P1) + Vector2Extensions.Cross(cp2.rB, P2));
#else
                            Vector2 d; d.X = x.X - a.X; d.Y = x.Y - a.Y;

                            // Apply incremental impulse
                            Vector2 P1; P1.X = d.X * normal.X; P1.Y = d.X * normal.Y;
                            Vector2 P2; P2.X = d.Y * normal.X; P2.Y = d.Y * normal.Y;
                            Vector2 P1P2; P1P2.X = P1.X + P2.X; P1P2.Y = P1.Y + P2.Y;

                            vA.X = vA.X - invMassA * P1P2.X; vA.Y = vA.Y - invMassA * P1P2.Y;
                            wA -= invIA * ((cp1.rA.X * P1.Y - cp1.rA.Y * P1.X) + (cp2.rA.X * P2.Y - cp2.rA.Y * P2.X));

                            vB.X = vB.X + invMassB * P1P2.X; vB.Y = vB.Y + invMassB * P1P2.Y;
                            wB += invIB * ((cp1.rB.X * P1.Y - cp1.rB.Y * P1.X) + (cp2.rB.X * P2.Y - cp2.rB.Y * P2.X));
#endif


                            // Accumulate
                            cp1.normalImpulse = x.X;
                            cp2.normalImpulse = x.Y;

        #if B2_DEBUG_SOLVER
					        // Postconditions
					        dv2 = vB + Vector2Extensions.Cross(wB, cp2.rB) - vA - Vector2Extensions.Cross(wA, cp2.rA);

					        // Compute normal velocity
					        vn2 = b2Dot(dv2, normal);

					        System.Diagnostics.Debug.Assert(b2Abs(vn2 - cp2.velocityBias) < k_errorTol);
        #endif

					        break;
				        }

				        //
				        // Case 4: x1 = 0 and x2 = 0
				        // 
				        // vn1 = b1
				        // vn2 = b2;
				        x.X = 0.0f;
				        x.Y = 0.0f;
				        vn1 = b.X;
				        vn2 = b.Y;

				        if (vn1 >= 0.0f && vn2 >= 0.0f)
				        {
					        // Resubstitute for the incremental impulse
#if !OPTIMIZE
                            Vector2 d = x - a;

                            // Apply incremental impulse
                            Vector2 P1 = d.X * normal;
                            Vector2 P2 = d.Y * normal;
                            vA -= invMassA * (P1 + P2);
                            wA -= invIA * (Vector2Extensions.Cross(cp1.rA, P1) + Vector2Extensions.Cross(cp2.rA, P2));

                            vB += invMassB * (P1 + P2);
                            wB += invIB * (Vector2Extensions.Cross(cp1.rB, P1) + Vector2Extensions.Cross(cp2.rB, P2));

#else
                            Vector2 d; d.X = x.X - a.X; d.Y = x.Y - a.Y;

                            // Apply incremental impulse
                            Vector2 P1; P1.X = d.X * normal.X; P1.Y = d.X * normal.Y;
                            Vector2 P2; P2.X = d.Y * normal.X; P2.Y = d.Y * normal.Y;
                            Vector2 P1P2; P1P2.X = P1.X + P2.X; P1P2.Y = P1.Y + P2.Y;

                            vA.X = vA.X - invMassA * P1P2.X; vA.Y = vA.Y - invMassA * P1P2.Y;
                            wA -= invIA * ((cp1.rA.X * P1.Y - cp1.rA.Y * P1.X) + (cp2.rA.X * P2.Y - cp2.rA.Y * P2.X));

                            vB.X = vB.X + invMassB * P1P2.X; vB.Y = vB.Y + invMassB * P1P2.Y;
                            wB += invIB * ((cp1.rB.X * P1.Y - cp1.rB.Y * P1.X) + (cp2.rB.X * P2.Y - cp2.rB.Y * P2.X));
#endif

                            // Accumulate
                            cp1.normalImpulse = x.X;
                            cp2.normalImpulse = x.Y;
                            
                            break;
				        }

				        // No solution, give up. This is hit sometimes, but it doesn't seem to matter.
				        break;
			        }

                    c.points[0] = cp1;
                    c.points[1] = cp2;
		        }

                m_constraints[i] = c;

		        bodyA.m_linearVelocity = vA;
		        bodyA.m_angularVelocity = wA;
		        bodyB.m_linearVelocity = vB;
		        bodyB.m_angularVelocity = wB;
	        }
        }

        public void StoreImpulses()
        {
            for (int i = 0; i < m_count; ++i)
            {
                ContactConstraint c = m_constraints[i];
                Manifold m = c.manifold;

                for (int j = 0; j < c.pointCount; ++j)
                {
                    var niJ = m.points[j];
                    var cpJ = c.points[j];

                    niJ.normalImpulse = cpJ.normalImpulse;
                    niJ.tangentImpulse = cpJ.tangentImpulse;

                    m.points[j] = niJ;
                    c.points[j] = cpJ;
                }

                // c.manifold = m; // not necessary -> Manifold is a class
                m_constraints[i] = c;
            }
        }

        PositionSolverManifold psm = new PositionSolverManifold();

        public bool SolvePositionConstraints(float baumgarte)
        {
            float minSeparation = 0.0f;

            for (int i = 0; i < m_count; ++i)
            {
                ContactConstraint c = m_constraints[i];
                Body bodyA = c.bodyA;
                Body bodyB = c.bodyB;

                float invMassA = bodyA.m_mass * bodyA.m_invMass;
                float invIA = bodyA.m_mass * bodyA.m_invI;
                float invMassB = bodyB.m_mass * bodyB.m_invMass;
                float invIB = bodyB.m_mass * bodyB.m_invI;

                // Solve normal constraints
                for (int j = 0; j < c.pointCount; ++j)
                {
                    //PositionSolverManifold psm = new PositionSolverManifold(ref c,j);
                    psm.Initialize(ref c, j);
                    Vector2 normal = psm.normal;

                    Vector2 point = psm.point;
                    float separation = psm.separation;

#if !OPTIMIZE
                    Vector2 rA = point - bodyA.m_sweep.c;
                    Vector2 rB = point - bodyB.m_sweep.c;

                    // Track max constraint error.
                    minSeparation = System.Math.Min(minSeparation, separation);

                    // Prevent large corrections and allow slop.
                    float C = MathHelper.Clamp(baumgarte * (separation + PhysicSettings.LinearSlop), -PhysicSettings.MaxLinearCorrection, 0.0f);

                    // Compute the effective mass.
                    float rnA = Vector2Extensions.Cross(rA, normal);
                    float rnB = Vector2Extensions.Cross(rB, normal);
#else
                    Vector2 rA; rA.X = point.X - bodyA.m_sweep.c.X; rA.Y = point.Y - bodyA.m_sweep.c.Y;
                    Vector2 rB; rB.X = point.X - bodyB.m_sweep.c.X; rB.Y = point.Y - bodyB.m_sweep.c.Y;

                    // Track max constraint error.
                    minSeparation = minSeparation < separation ? minSeparation : separation;

                    // Prevent large corrections and allow slop.
                    float C = MathHelper.Clamp(baumgarte * (separation + PhysicSettings.LinearSlop), -PhysicSettings.MaxLinearCorrection, 0.0f);

                    // Compute the effective mass.
                    float rnA = rA.X * normal.Y - rA.Y * normal.X;
                    float rnB = rB.X * normal.Y - rB.Y * normal.X;
#endif
                    float K = invMassA + invMassB + invIA * rnA * rnA + invIB * rnB * rnB;


                    // Compute normal impulse
                    float impulse = K > 0.0f ? -C / K : 0.0f;

#if !OPTIMIZE
                    Vector2 P = impulse * normal;

                    bodyA.m_sweep.c -= invMassA * P;
                    bodyA.m_sweep.a -= invIA * Vector2Extensions.Cross(rA, P);
                    bodyA.SynchronizeTransform();

                    bodyB.m_sweep.c += invMassB * P;
                    bodyB.m_sweep.a += invIB * Vector2Extensions.Cross(rB, P);
                    bodyB.SynchronizeTransform();
#else
                    Vector2 P; P.X = impulse * normal.X; P.Y = impulse * normal.Y;

                    bodyA.m_sweep.c.X = bodyA.m_sweep.c.X - invMassA * P.X;
                    bodyA.m_sweep.c.Y = bodyA.m_sweep.c.Y - invMassA * P.Y;
                    bodyA.m_sweep.a = bodyA.m_sweep.a - invIA * (rA.X * P.Y - rA.Y * P.X);
                    bodyA.SynchronizeTransform();

                    bodyB.m_sweep.c.X = bodyB.m_sweep.c.X + invMassB * P.X;
                    bodyB.m_sweep.c.Y = bodyB.m_sweep.c.Y + invMassB * P.Y;
                    bodyB.m_sweep.a = bodyB.m_sweep.a + invIB * (rB.X * P.Y - rB.Y * P.X);
                    bodyB.SynchronizeTransform();
#endif
                }

                m_constraints[i] = c;
            }

            // We can't expect minSpeparation >= -b2_linearSlop because we don't
            // push the separation above -b2_linearSlop.
            return minSeparation >= LinearSlop15f;
        }

        // Sequential position solver for position constraints.
        public bool SolveTOIPositionConstraints(float baumgarte, Body toiBodyA, Body toiBodyB)
        {
            float minSeparation = 0.0f;

            for (int i = 0; i < m_count; ++i)
            {
                ContactConstraint c = m_constraints[i];
                Body bodyA = c.bodyA;
                Body bodyB = c.bodyB;

                float massA = 0.0f;
                if (bodyA == toiBodyA || bodyA == toiBodyB)
                {
                    massA = bodyA.m_mass;
                }

                float massB = 0.0f;
                if (bodyB == toiBodyA || bodyB == toiBodyB)
                {
                    massB = bodyB.m_mass;
                }

                float invMassA = bodyA.m_mass * bodyA.m_invMass;
                float invIA = bodyA.m_mass * bodyA.m_invI;
                float invMassB = bodyB.m_mass * bodyB.m_invMass;
                float invIB = bodyB.m_mass * bodyB.m_invI;

                // Solve normal vonstraints
                PositionSolverManifold psm = new PositionSolverManifold();
                for (int j = 0; j < c.pointCount; ++j)
                {
                    psm.Initialize(ref c, j);
                    Vector2 normal = psm.normal;

                    Vector2 point = psm.point;
                    float separation = psm.separation;

                    Vector2 rA = point - bodyA.m_sweep.c;
                    Vector2 rB = point - bodyB.m_sweep.c;

                    // Track max constraint error.
                    minSeparation = System.Math.Min(minSeparation, separation);

                    // Prevent large corrections and allow slop.
                    float C = MathHelper.Clamp(baumgarte * (separation + PhysicSettings.LinearSlop), -PhysicSettings.MaxLinearCorrection, 0.0f);

                    // Compute the effective mass.
                    float rnA = Vector2Extensions.Cross(rA, normal);
                    float rnB = Vector2Extensions.Cross(rB, normal);
                    float K = invMassA + invMassB + invIA * rnA * rnA + invIB * rnB * rnB;

                    // Compute normal impulse
                    float impulse = K > 0.0f ? - C / K : 0.0f;

                    Vector2 P = impulse * normal;

                    bodyA.m_sweep.c -= invMassA * P;
                    bodyA.m_sweep.a -= invIA * Vector2Extensions.Cross(rA, P);
                    bodyA.SynchronizeTransform();

                    bodyB.m_sweep.c += invMassB * P;
                    bodyB.m_sweep.a += invIB * Vector2Extensions.Cross(rB, P);
                    bodyB.SynchronizeTransform();
                }

                m_constraints[i] = c;
            }

            // We can't expect minSpeparation >= -b2_linearSlop because we don't
            // push the separation above -b2_linearSlop.
            return minSeparation >= -1.5f * PhysicSettings.LinearSlop;
        }

        const float LinearSlop15f = -1.5f * PhysicSettings.LinearSlop;

	    public TimeStep m_step;
	    public ContactConstraint[] m_constraints;
	    public int m_count;
    };

    public class PositionSolverManifold
    {
        public void Initialize(ref ContactConstraint cc, int index)
        {
            System.Diagnostics.Debug.Assert(cc.pointCount > 0);

            switch (cc.type)
            {
                case ManifoldType.e_circles:
                    {
                        Vector2 pointA = cc.bodyA.GetWorldPoint(ref cc.localPoint);
                        Vector2 pointB = cc.bodyB.GetWorldPoint(cc.points[0].localPoint);
                        float dstSqr; Vector2.DistanceSquared(ref pointA, ref pointB, out dstSqr);
                        if (dstSqr > PhysicSettings.FLT_EPSILON_SQUARED)
                        {
                            normal = pointB - pointA;
                            normal.Normalize();
                        }
                        else
                        {
                            normal = Vector2.UnitX;
                        }

                        point = 0.5f * (pointA + pointB);
                        separation = Vector2.Dot(pointB - pointA, normal) - cc.radiusA - cc.radiusB;
                    }
                    break;

                case ManifoldType.e_faceA:
                    {
                        normal = cc.bodyA.GetWorldVector(cc.localNormal);
                        Vector2 planePoint = cc.bodyA.GetWorldPoint(ref cc.localPoint);

                        Vector2 clipPoint = cc.bodyB.GetWorldPoint(cc.points[index].localPoint);
                        separation = Vector2.Dot(clipPoint - planePoint, normal) - cc.radiusA - cc.radiusB;
                        point = clipPoint;
                    }
                    break;

                case ManifoldType.e_faceB:
                    {
                        normal = cc.bodyB.GetWorldVector(cc.localNormal);
                        Vector2 planePoint = cc.bodyB.GetWorldPoint(ref cc.localPoint);

                        Vector2 clipPoint = cc.bodyA.GetWorldPoint(cc.points[index].localPoint);
                        separation = Vector2.Dot(clipPoint - planePoint, normal) - cc.radiusA - cc.radiusB;
                        point = clipPoint;

                        // Ensure normal points from A to B
                        normal = -normal;
                    }
                    break;
                default:
                    throw new Exception("PositionSolverManifold.Initialize");
            }
        }

        public Vector2 normal;
        public Vector2 point;
        public float separation;
    };
}
